from pyModbusTCP.client import ModbusClient
import time

class SixArm:
    REGISTERS_COUNT = 1
    ELEMENTS_COUNT = 0
    INPUT_ADDR = 0x0000
    STATUS_ADDR = 0x0001

    TRANSFER_BLOCK_FROM_CAR_TO_PRINTER_INSTRUCT = 0
    TRANSFER_BLOCK_PRINTER_CAT_TO_CAR_INSTRUCT = 1

    TRANSFER_BLOCK_FROM_CAR_TO_PRINTER_START = 1
    TRANSFER_BLOCK_FROM_CAR_TO_PRINTER_SUCCESS = 2
    TRANSFER_BLOCK_PRINTER_CAT_TO_CAR_START = 3
    TRANSFER_BLOCK_PRINTER_CAT_TO_CAR_SUCCESS = 0

    TIMEOUT_SECONDS = 50

    def __init__(self, host, port):
        self.host = host
        self.port = port
        self.state = -1
        self.client = ModbusClient(host=self.host, port=self.port)

    def initialize_robot(self):
        # 打开连接
        if not self.client.open():
            print("无法连接到Modbus设备")
    
    def __check_status_and_print_message(self, target_state):
        start_time = time.time()
        self.state = self.client.read_holding_registers(self.STATUS_ADDR, self.REGISTERS_COUNT)[self.ELEMENTS_COUNT]
        while self.state != target_state :
            time.sleep(1)
            self.state = self.client.read_holding_registers(self.STATUS_ADDR, self.REGISTERS_COUNT)[self.ELEMENTS_COUNT]
            elapsed_time = time.time() - start_time
            if elapsed_time > self.TIMEOUT_SECONDS:
                print("等待超时")
                return False
        return True

    def transfer_block_from_car_to_printer(self):
        self.client.write_single_register(self.INPUT_ADDR, self.TRANSFER_BLOCK_FROM_CAR_TO_PRINTER_INSTRUCT)

        if self.__check_status_and_print_message(self.TRANSFER_BLOCK_FROM_CAR_TO_PRINTER_START):
            print("开始取料")
            if self.__check_status_and_print_message(self.TRANSFER_BLOCK_FROM_CAR_TO_PRINTER_SUCCESS):
                print("取料结束")


    def transfer_block_from_printer_to_car(self):
        self.client.write_single_register(self.INPUT_ADDR, self.TRANSFER_BLOCK_PRINTER_CAT_TO_CAR_INSTRUCT)
        
        if self.__check_status_and_print_message(self.TRANSFER_BLOCK_PRINTER_CAT_TO_CAR_START):
            print("开始放料")
            if self.__check_status_and_print_message(self.TRANSFER_BLOCK_PRINTER_CAT_TO_CAR_SUCCESS):
                print("放料结束")


    def disconnect_robot(self):
        # 关闭连接
        self.client.close()


if __name__ == '__main__':
    # 示例：先取料，动作结束后过5秒放料
    host = "192.168.1.30"
    port = 502
    six_arm = SixArm(host, port)
    six_arm.initialize_robot()
    six_arm.transfer_block_from_car_to_printer()
    time.sleep(5)
    six_arm.transfer_block_from_printer_to_car()
    six_arm.disconnect_robot()